#ifndef _Robot_h__
#define _Robot_h__



#include <vector>
#include <deque>

#include <boost/numeric/ublas/matrix.hpp>




#include "position_solutions.h"

using namespace boost::numeric;

const int BONES_COUNT=5;

struct model
{
  SoSeparator  *bones[BONES_COUNT];
  SoSeparator  *model;
  
  
  SoSeparator get_result();
  
  // MAV_composite arm_closed,arm_opened;  I don't remeber what is it...
  //MAV_object *arm_closed_obj, *arm_opened_obj;
  
  matr origin;
  // MAV_matrix m;
  bool load(matr Origin=ublas::identity_matrix<double>(4));
  void orient(std::vector<double> Q);

  void set_closed();
  void set_opened();
  
};


class Robot
{
public:
  Robot(std::vector<double> Q= std::vector<double>(6,0));

  void set_speed(double val);

  void gos(double x, double y, double z, double o, double a, double t);
  void gos(std::vector<double> P);
  void gos(matr dest);

  void go(std::vector<double> Q);

  void gosnear(std::vector<double> P,double shift);
    
  void move(double x, double y, double z, double o, double a, double t);
  void move(std::vector<double> P);

  void open();
  void close();

  void j5_plus(){ W=Plus; }
  
      
  void j5_minus(){ W=Minus;}
  
  
		     
      
  
  
  std::vector<double> xyz();
  
    
  void clear_path();
  bool traverse();

  model arm;
private:
  std::deque< std::vector <double> > path_points;
  modifier k1,k2,W;
  double speed;
};


#endif
